#include "motor.h"

// index = GPIOn_IOx = (n-1)*32 + x
// 例如蜂鸣器使用的引脚编号为：index = GPIO1_19 = (1-1)*32 +19 = 19。
// 又例如 GPIO4_IO20 的编号为：index = GPIO4_IO20 = (4-1)*32+20=116

//  IO4_19
#define Motor_pwm_export    "/sys/class/pwm/pwmchip6/export"
#define Motor_pwm_PATH      "/sys/class/pwm/pwmchip6/pwm0/"

// #define Motor_period            "1000000"
// #define Motor_duty              "500000"
#define Stop_sig_gpio_num     "0"
#define Motor_dire_gpio_num       "2"
// #define Motor_en_gpio_num       "3"

extern sem_t sem_motor;

int motor_init(void)
{
    int ret;
    int fd_dire;
    int fd_sig;
    int fd_duty;
    // int fd_enable;
    

    /************************ Motor **************************/
    /*接口导出到用户空间*/
    if( access(Motor_pwm_PATH, F_OK) != 0 ){
        ret = open(Motor_pwm_export, O_WRONLY);
        if( ret < 0 ){
            printf("Motor_pwm_export open error\n");
            // return -1;
            exit(-1);
        }
        if( write(ret, "0", 1) != 1 ){
            printf("Motor_pwm_export export write error\n");
            // return -1;
            exit(-1);
        }
    }

    /*设置PWM频率*/
    ret = open(Motor_pwm_PATH"period", O_WRONLY);
    if( ret < 0 ){
        printf("Motor_pwm_PATH period open error\n");
        // return -1;
        exit(-1);
    }
    if( write(ret, "5000000", 7) != 7 ){
        printf("Motor_pwm_PATH  period write error\n");
        // return -1;
        exit(-1);
    }
    /*设置PWM占空比*/
    fd_duty = open(Motor_pwm_PATH"duty_cycle", O_WRONLY);
    if( fd_duty < 0 ){
        printf("Motor_pwm_PATH duty_cycle open error\n");
        // return -1;
        exit(-1);
    }
    if( write(fd_duty, "5000000", 7) != 7 ){
        printf("Motor_pwm_PATH duty_cycle write error\n");
        // return -1;
        exit(-1);
    }
    /*设置PWM极性*/
    ret = open(Motor_pwm_PATH"polarity", O_WRONLY);
    if( ret < 0 ){
        printf("Motor_pwm_PATH polarity open error\n");
        // return -1;
        exit(-1);        
    }
    if( write(ret, "normal", sizeof("normal")) != sizeof("normal") ){
        printf("Motor_pwm_PATH polarity write error\n");
        // return -1;
        exit(-1);
    }
    /*使能PWM*/
    ret = open(Motor_pwm_PATH"enable", O_WRONLY);
    if( ret < 0 ){
        printf("Motor_pwm_PATH enable open error\n");
        // return -1;
        exit(-1);
    }
    if( write(ret, "1", 1) != 1 ){
        printf("Motor_pwm_PATH enable write error\n");
        // return -1;
        exit(-1);
    }
    close(ret);
    /************************ Motor_dire_gpio_num **************************/
        /* 导出io口节点到用户空间*/
    if( access("/sys/class/gpio/gpio" Motor_dire_gpio_num, F_OK) != 0 ){
        fd_dire = open("/sys/class/gpio/export", O_WRONLY);
        if( fd_dire < 0 ){
            printf("Motor_dire_gpio_num export open error\n");
            // return -1;
            exit(-1);
        }
        if( write(fd_dire, Motor_dire_gpio_num, sizeof(Motor_dire_gpio_num)) != sizeof(Motor_dire_gpio_num) ){
            printf("Motor_dire_gpio_num write export error\n");
            return -1;
            exit(-1);
        }
    }
    
    /*设置io口为输出模式*/
    fd_dire = open("/sys/class/gpio/gpio" Motor_dire_gpio_num "/direction", O_WRONLY);
    if( fd_dire < 0 ){
        printf("Motor_dire_gpio_num direction open error\n");
        // return -1;
        exit(-1);
    }
    if( write(fd_dire, "out", 3) != 3 ){
        printf("Motor_dire_gpio_num write direction error\n");
        // return -1;
        exit(-1);
    }
    /*设置io口电平*/
    fd_dire = open("/sys/class/gpio/gpio" Motor_dire_gpio_num "/value", O_WRONLY);
    if( fd_dire < 0 ){
        printf("Motor_dire_gpio_num value open error\n");
        // return -1;
        exit(-1);
    }

    /*********************** Stop_sig_gpio_num ***************************/
        /* 导出io口节点到用户空间*/
    if( access("/sys/class/gpio/gpio"Stop_sig_gpio_num, F_OK) != 0 ){
        fd_sig = open("/sys/class/gpio/export", O_WRONLY);
        if( fd_sig < 0 ){
            printf("Stop_sig_gpio_num export open error\n");
            // return -1;
            exit(-1);
        }
        if( write(fd_sig, Stop_sig_gpio_num, sizeof(Stop_sig_gpio_num)) != sizeof(Stop_sig_gpio_num) ){
            printf("Stop_sig_gpio_num write export error\n");
            // return -1;
            exit(-1);
        }
    }
    /*设置io口为输入模式*/
    fd_sig = open("/sys/class/gpio/gpio"Stop_sig_gpio_num"/direction", O_WRONLY);
    if( fd_sig < 0 ){
        printf("Stop_sig_gpio_num direction open error\n");
        return -1;
    }
    if( write(fd_sig, "in", 2) != 2 ){
        printf("Stop_sig_gpio_num write direction error\n");
        // return -1;
        exit(-1);
    }
    /*设置io口电平*/
    fd_sig = open("/sys/class/gpio/gpio" Stop_sig_gpio_num "/value", O_RDONLY);
    if( fd_sig < 0 ){
        printf("Stop_sig_gpio_num value open error\n");
        // return -1;
        exit(-1);
    }

    /*********************** Motor_en_gpio_num ***************************/
    //     /* 导出io口节点到用户空间*/
    // if( access("/sys/class/gpio/gpio" Motor_en_gpio_num, F_OK) != 0 ){
    //     fd_enable = open("/sys/class/gpio/export", O_WRONLY);
    //     if( fd_enable < 0 ){
    //     printf("Motor_en_gpio_num export open error\n");
    //     return -1;
    //     }
    //     if( write(fd_enable, Motor_en_gpio_num, sizeof(Motor_en_gpio_num)) != sizeof(Motor_en_gpio_num) ){
    //     printf("Motor_en_gpio_num write export error\n");
    //     return -1;
    //     }
    // }
    // /*设置io口为输出模式*/
    // fd_enable = open("/sys/class/gpio/gpio" Motor_en_gpio_num "/direction", O_WRONLY);
    // if( fd_enable < 0 ){
    //     printf("Motor_en_gpio_num direction open error\n");
    //     return -1;
    // }
    // if( write(fd_enable, "out", 3) != 3 ){
    //     printf("Motor_en_gpio_num write direction error\n");
    //     return -1;
    // }
    // /*设置io口电平*/
    // fd_enable = open("/sys/class/gpio/gpio" Motor_en_gpio_num "/value", O_WRONLY);
    // if( fd_enable < 0 ){
    //     printf("Motor_en_gpio_num value open error\n");
    //     return -1;
    // }

    Motor_dire(fd_dire, 1);
    // Motor_enable(fd_enable, 0);
    printf("motor program is ready\n");
    sem_post(&sem_motor);
    usleep(10*1000);
    while(1){

        sem_wait(&sem_motor);
        // if( read(fd_sig, &sig_vol, 1) > 0 ){
        //     printf("%c\n", sig_vol);
        //     lseek(fd_sig, SEEK_SET, 0);
        // }
        // sleep(1);
        Motor_dire(fd_dire, 1);
        if( write(fd_duty, "500000", 6) != 6 ){
            if( write(fd_duty, "500000", 6) != 6 ){
                printf("set Motor_dire error\n");
            }
        }
        printf("motor run!  dir 1\n");
        usleep(50*1000);
        sem_wait(&sem_motor);
        Motor_dire(fd_dire, 0);
        printf("motor run!  dir 0\n");
        usleep(1000*1000);
        while (1)
        {
            // if( read(fd_sig, &sig_vol, 1) > 0 ){
            //     printf(" sig_vol = %c\n", sig_vol);
            //     if( sig_vol == '1' ){
            //         if( write(fd_duty, "5000000", 7) != 7 ){
            //             if( write(fd_duty, "5000000", 7) != 7 ){
            //                 printf("set Motor_dire error\n");
            //             }
            //         }
            //         lseek(fd_sig, SEEK_SET, 0);
            //         break;
            //     }
            ret = Sig_read(fd_sig);
            printf(" sig_vol = %d\n", ret);
            if( ret == 1 ){
                if( write(fd_duty, "5000000", 7) != 7 ){
                    if( write(fd_duty, "5000000", 7) != 7 ){
                        printf("set Motor_dire error\n");
                        exit(-1);
                    }
                }
                break;
            }
            usleep(30*1000);
        }
        
        
        usleep(50*1000);
    }

}

int Sig_read(int fd)
{
    u_int8_t sig_vol;
    if( read(fd, &sig_vol, 1) > 0 ){
        if( sig_vol == '0' ){
            lseek(fd, SEEK_SET, 0);
            return 0;
        }
        else if( sig_vol == '1' ){
            lseek(fd, SEEK_SET, 0);
            return 1;
        }
        else
            return -1;
    }
    return -1;
}

void Motor_dire(int fd, u_int8_t dir)
{
    if( dir == 0 ){
        if( write(fd, "0", 1) != 1 ){
            printf("set Motor_dire error\n");
        }
    }
    else if( dir == 1 ){
        if( write(fd, "1", 1) != 1 ){
            printf("set Motor_dire error\n");
        }
    }
}

void Motor_enable(int fd, u_int8_t dir)
{
    //     if( dir == 0 ){
    //     if( write(fd, "0", 1) != 1 ){
    //         printf("set Motor_enable error\n");
    //     }
    // }
    // else if( dir == 1 ){
    //     if( write(fd, "1", 1) != 1 ){
    //         printf("set Motor_enable error\n");
    //     }
    // }
}
